package owg.util.euclidian;

import owg.util.Calc;

/**
 * This is a rigid 3D vector class for abstracting the representation and computing of a wide range of values. 
 * Many of the methods are designed so to not allocate any heap space;
 * Excercise caution so you do not accidentally overwrite your vectors. 
 * In addition, many of the methods return the vector on which the call was made, so the calls can be chained.
 */
public class V3D implements VXD
	{
	public static final char[] COMPONENT_NAMES = {'x','y','z'};
	//Some standard vectors. Be very careful not to alter these. 
	//If assertions are enabled, they will be checked periodically for correctness.
	public static final V3D ZERO = new V3D();
	
	public static final V3D X = new V3D(1,0,0);
	public static final V3D Y = new V3D(0,1,0);
	public static final V3D Z = new V3D(0,0,1);
	
	public static final V3D NEG_X = new V3D(-1,0,0);
	public static final V3D NEG_Y = new V3D(0,-1,0);
	public static final V3D NEG_Z = new V3D(0,0,-1);
	
	public static boolean testStandardVectors()
		{
		if(!ZERO.isZero())
			throw new RuntimeException("Fail");
		V3D tmp = new V3D();
		if(!X.equals(tmp.set(1,0,0)))
			throw new RuntimeException("Fail");
		if(!Y.equals(tmp.set(0,1,0)))
			throw new RuntimeException("Fail");
		if(!Z.equals(tmp.set(0,0,1)))
			throw new RuntimeException("Fail");
		if(!NEG_X.equals(tmp.set(-1,0,0)))
			throw new RuntimeException("Fail");
		if(!NEG_Y.equals(tmp.set(0,-1,0)))
			throw new RuntimeException("Fail");
		if(!NEG_Z.equals(tmp.set(0,0,-1)))
			throw new RuntimeException("Fail");
		
		return true;
		}
	
	public double[] p = new double[3];
	/**
	 * Construct with zero length
	 */
	public V3D()
		{
		this(0,0,0);
		}
	/**
	 * Construct with the specified components
	 */
	public V3D(double x,double y,double z)
		{
		p[0]=x;
		p[1]=y;
		p[2]=z;
		}
	/**
	 * Construct from the float array, starting from the offset 
	 */
	public V3D(float[] vector, int offset)
		{
		this((double)vector[offset],(double)vector[offset+1],(double)vector[offset+2]);
		}
	/**
	 * Construct from the double array, starting from the offset 
	 */
	public V3D(double[] vector, int offset)
		{
		this((double)vector[offset],(double)vector[offset+1],(double)vector[offset+2]);
		}

	/**
	 * Construct from the byte array, starting from the offset 
	 */
	public V3D(byte[] vector, int offset)
		{
		this((double)vector[offset],(double)vector[offset+1],(double)vector[offset+2]);
		}
	/**
	 * Set all components
	 * @return this vector
	 */
	public V3D set(double x,double y,double z)
		{
		p[0]=x;
		p[1]=y;
		p[2]=z;
		return this;
		}
	/**
	 * Set all components
	 * @return this vector
	 */
	public V3D set(V3D source)
		{
		p[0]=source.p[0];
		p[1]=source.p[1];
		p[2]=source.p[2];
		return this;
		}
	/**
	 * Set all components
	 * @return this vector
	 */
	public V3D set(float[] src)
		{
		p[0]=(double)src[0];
		p[1]=(double)src[1];
		p[2]=(double)src[2];
		return this;
		}
	/**
	 * Set all components
	 * @return this vector
	 */
	public V3D set(double [] src)
		{
		p[0]=(double)src[0];
		p[1]=(double)src[1];
		p[2]=(double)src[2];
		return this;
		}

	/**
	 * Calculate the length
	 */
	public double len()
		{
		return (double)Math.sqrt(Math.pow(p[0],2)+Math.pow(p[1],2)+Math.pow(p[2],2));
		}
	/**Calculate the length of this-other*/
	public double distance(V3D other)
		{
		return (double)Math.sqrt(Math.pow(p[0]-other.p[0],2)+Math.pow(p[1]-other.p[1],2)+Math.pow(p[2]-other.p[2],2));
		}
	/**Calculate the squared length of this-other*/
	public double sqDistance(V3D other)
		{
		return (double)(Math.pow(p[0]-other.p[0],2)+Math.pow(p[1]-other.p[1],2)+Math.pow(p[2]-other.p[2],2));
		}
	/**Calculate the angle(radians) between this and other*/
	public double angle(V3D other)
		{
		return (double)Calc.acos(dot(other)/(len()*other.len()));
		}
	
	/**
	 * Make the length equal to 1 while keeping the direction.
	 * Makes the vector (1,0,0) if length is 0.
	 * @return this vector
	 */
	public V3D normalize()
		{
		double d = (double)(Math.sqrt(Math.pow(p[0],2)+Math.pow(p[1],2)+Math.pow(p[2],2)));
		if (d==0)
			{
			p[0]=1;
			p[1]=0;
			p[2]=0;
			}
		else
			{
			p[0]/=d;
			p[1]/=d;
			p[2]/=d;
			}
		return this;
		}
	/**
	 * Multiply all components with the value
	 * @return this vector
	 */
	public V3D multiply(double m)
		{
		p[0]*=m;
		p[1]*=m;
		p[2]*=m;
		return this;
		}
	/**
	 * Multiply all components with the respective values
	 * @return this vector
	 */
	public V3D multiply(double mx, double my, double mz)
		{
		p[0]*=mx;
		p[1]*=my;
		p[2]*=mz;
		return this;
		}
	/**
	 * Add the components to the current values
	 * @return this vector
	 */
	public V3D add(double x,double y,double z)
		{
		p[0]+=x;
		p[1]+=y;
		p[2]+=z;
		return this;
		}
	/**
	 * Add the vector's components to the current values. 
	 * This method allows the input to be null.
	 * @return this vector
	 */
	public V3D addAllowNull(V3D v)
		{
		if(v!=null)
			{
			p[0]+=v.p[0];
			p[1]+=v.p[1];
			p[2]+=v.p[2];
			}
		return this;
		}

	/**
	 * Add the vector's components to the current values
	 * @return this vector
	 */
	public V3D add(V3D v)
		{
		p[0]+=v.p[0];
		p[1]+=v.p[1];
		p[2]+=v.p[2];
		return this;
		}
	/**
	 * Subtract the vector's components from the current values
	 * @return this vector
	 */
	public V3D sub(V3D v)
		{
		p[0]-=v.p[0];
		p[1]-=v.p[1];
		p[2]-=v.p[2];
		return this;
		}
	/**
	 * Add the vector multiplied with the value
	 * @return this vector
	 */
	public V3D add(V3D v,double multiplicator)
		{
		p[0]+=multiplicator*v.p[0];
		p[1]+=multiplicator*v.p[1];
		p[2]+=multiplicator*v.p[2];
		return this;
		}
	
	/**
	 * Set this vector to the cross product of the specified vectors.
	 * Warning: It is unsafe to specify this vector as one of the arguments.
	 * @return this vector
	 */
	public V3D setCross(V3D v1, V3D v2)
		{
		if(v1==this || v2==this) throw new IllegalArgumentException("Invalid cross product arguments...");
		p[0]=v1.p[1]*v2.p[2]-v1.p[2]*v2.p[1];
		p[1]=v1.p[2]*v2.p[0]-v1.p[0]*v2.p[2];
		p[2]=v1.p[0]*v2.p[1]-v1.p[1]*v2.p[0];
		return this;
		}
	/**
	 * Set this vector to the cross product of this and the specified vector.
	 * @return this vector
	 */
	public V3D setCross(V3D v2)
		{
		double x=p[0], y=p[1], z=p[2];
		p[0]=y*v2.p[2]-z*v2.p[1];
		p[1]=z*v2.p[0]-x*v2.p[2];
		p[2]=x*v2.p[1]-y*v2.p[0];
		return this;
		}
	/**
	 * Return a new vector which is the cross product of this and the specified vector
	 */
	public V3D createCross(V3D v)
		{
		double x,y,z;
		x=p[1]*v.p[2]-p[2]*v.p[1];
		y=p[2]*v.p[0]-p[0]*v.p[2];
		z=p[0]*v.p[1]-p[1]*v.p[0];
		return new V3D(x,y,z);
		}

	/**
	 * Return a new vector which is the normal of the ccw plane consisting of this, a, b.
	 */
	public V3D createNormal(V3D a, V3D b)
		{
		return b.clone().sub(this).setCross(a.clone().sub(this)).normalize();
		}
	/**
	 * Make this vector the average of this and the specified vector(weight=0 -> this, weight=1 -> other)
	 * @return this vector
	 */
	public V3D makeAverage(V3D other,double weight)
		{
		p[0]=(p[0]*(1-weight)+other.p[0]*weight);
		p[1]=(p[1]*(1-weight)+other.p[1]*weight);
		p[2]=(p[2]*(1-weight)+other.p[2]*weight);
		return this;
		}

	/**
	 * Make this vector the average of this and the specified components(weight=0 -> this, weight=1 -> components)
	 * @return this vector
	 */
	public V3D makeAverage(double x, double y, double z, double weight)
		{
		p[0]=(p[0]*(1-weight)+x*weight);
		p[1]=(p[1]*(1-weight)+y*weight);
		p[2]=(p[2]*(1-weight)+z*weight);
		return this;
		}
	/**
	 * Return the dot product of this and the specified vector
	 */
	public double dot(V3D v)
		{
		return p[0]*v.p[0]+p[1]*v.p[1]+p[2]*v.p[2];
		}
	/**
	 * Return the dot product of this and the specified vector
	 */
	public double dot(double x,double y,double z)
		{
		return p[0]*x+p[1]*y+p[2]*z;
		}
	/**
	 * Return the x-component
	 */
	public double x()
		{
		return p[0];
		}
	/**
	 * Return the y-component
	 */
	public double y()
		{
		return p[1];
		}
	/**
	 * Return the z-component
	 */
	public double z()
		{
		return p[2];
		}
	/**
	 * Return the backing array
	 */
	public double[] array()
		{
		return p;
		}
	/**
	 * Set the x-component
	 * @return this vector
	 */
	public V3D x(double x)
		{
		p[0]=x;
		return this;
		}
	/**
	 * Set the y-component
	 * @return this vector
	 */
	public V3D y(double y)
		{
		p[1]=y;
		return this;
		}
	/**
	 * Set the z-component
	 * @return this vector
	 */
	public V3D z(double z)
		{
		p[2]=z;
		return this;
		}
	/**
	 * Return a new float array representing this vector
	 * Note that the p member of this class can be used directly if the type matches and a copy is not needed.
	 */
	public float[] toFloat()
		{
		float[] result = {(float)p[0],(float)p[1],(float)p[2]};
		return result;
		}
	/**Insert 3 elements into the given array starting from the indicated index,
	 * representing the x, y and z value of this vector.*/
	public void toFloat(float[] vertices, int i)
		{
		vertices[i+0] = (float)p[0];
		vertices[i+1] = (float)p[1];
		vertices[i+2] = (float)p[2];
		}
	/**
	 * Return a new double array representing this vector.
	 * Note that the p member of this class can be used directly if the type matches and a copy is not needed.
	 */
	public double[] toDouble()
		{
		double[] result = {(double)p[0],(double)p[1],(double)p[2]};
		return result;
		}
	/**Insert 3 elements into the given array starting from the indicated index,
	 * representing the x, y and z value of this vector.*/
	public void toDouble(double[] vertices, int i)
		{
		vertices[i+0] = (double)p[0];
		vertices[i+1] = (double)p[1];
		vertices[i+2] = (double)p[2];
		}
	/**
	 * Return a signed byte array representing this vector
	 */
	public byte[] toByte()
		{
		byte[] result = {(byte)p[0],(byte)p[1],(byte)p[2]};
		return result;
		}
	/**
	 * Return a signed short array representing this vector
	 */
	public short[] toShort()
		{
		short[] result = {(short)p[0],(short)p[1],(short)p[2]};
		return result;
		}
	/**
	 * Return a signed int array representing this vector
	 */
	public int[] toInt()
		{
		int[] result = {(int)p[0],(int)p[1],(int)p[2]};
		return result;
		}
	/**
	 * Return a signed long array representing this vector
	 */
	public long[] toLong()
		{
		long[] result = {(long)p[0],(long)p[1],(long)p[2]};
		return result;
		}
	/**
	 * Make a copy
	 */
	@Override
	public V3D clone()
		{
		return new V3D(p[0],p[1],p[2]);
		}
	/**Return a new V3D with the values from this vector*/
	public V3D asV3D()
		{
		return new V3D((double)p[0], (double)p[1], (double)p[2]);
		}
		
	/**Return a new V3F with the values from this vector*/
	public V3F asV3F()
		{
		return new V3F((float)p[0], (float)p[1], (float)p[2]);
		}
	/**Return a new V2D with the values from this vector. The z component is discarded.*/
	public V2D asV2D()
		{
		return new V2D((double)p[0], (double)p[1]);
		}
		
	/**Return a new V2F with the values from this vector. The z component is discarded.*/
	public V2F asV2F()
		{
		return new V2F((float)p[0], (float)p[1]);
		}
	/**
	 * Print the coordinates. 
	 */
	@Override
	public  String toString()
		{
		return "x:"+p[0]+", y:"+p[1]+", z:"+p[2]+" ";
		}
	/**
	 * Parse a string into a V3D. The String should correspond to the output from {@link V3D#toString()}
	 * @param str A string in the form<br/> 
	 * x:[x],y:[y],z:[z]<br/>
	 * where [x] is the numerical x coordinate value and soforth. Alternatively, the identifiers (x:) may be omitted.
	 * The values must appear in order in any case.
	 * @return A V3D object with the values defined in the string.
	 */
	public static V3D parseV3D(String str) throws IllegalArgumentException
		{
		return new V3D(str);
		}
	/**
	 * Parse a string into a V3D. The String should correspond to the output from {@link V3D#toString()}
	 * @param str A string in the form<br/> 
	 * x:[x],y:[y],z:[z]<br/>
	 * where [x] is the numerical x coordinate value and soforth. Alternatively, the identifiers (x:) may be omitted.
	 * The values must appear in order in any case.
	 */
	public V3D(String str) throws IllegalArgumentException
		{
		String[] in = str.split(",");
		if(in.length < 3)
			throw new IllegalArgumentException("Vector must have 3 components. Found: "+in.length+" in "+str);
		try
			{
			for(int i = 0; i<3; i++)
				{
				int pos = in[i].lastIndexOf(':');
				if(pos == -1)
					p[i] = Double.parseDouble(in[i]);
				else
					p[i] = Double.parseDouble(in[i].substring(pos+1));
				}
			}
		catch (NumberFormatException e)
			{
			throw new IllegalArgumentException("Error in parsing "+str, e);
			}
		}
	
		
	/**
	 * Accelerate in the current direction by an absolute value. 
	 * If length is under the negative of the value, the vector is set to zero.
	 * @return this vector*/
	public V3D accelerate(double d)
		{
		double sp = len();
		if(sp!=0)
			{
			if (sp<=-d)
				{
				p[0]=0; p[1]=0; p[2]=0;
				return this;
				}
			d=1+d/sp;
			p[0]*=d; p[1]*=d; p[2]*=d;
			}
		return this;
		}
	/**
	 * Accelerate towards becoming identical to other by an absolute value.
	 * @param other the vector to move towards
	 * @param the amount to increment this vector by, in the direction of other. Non-positive values have no effect.
	 * @return this vector*/
	public V3D accelerateTowards(V3D other, double d)
		{
		if(d <= 0)
			return this;
		double sp = distance(other);
		if (sp<=d)
			{
			p[0]=other.p[0]; p[1]=other.p[1]; p[2]=other.p[2];
			return this;
			}
		d=d/sp;
		p[0]=p[0]*(1-d)+other.p[0]*(d); 
		p[1]=p[1]*(1-d)+other.p[1]*(d); 
		p[2]=p[2]*(1-d)+other.p[2]*(d);
		return this;
		}
	/**
	 * Calculate the squared length. Cheaper than calculating length since there is no square root.
	 */
	public double sqLen()
		{
		return (double)(Math.pow(p[0],2)+Math.pow(p[1],2)+Math.pow(p[2],2));
		}
	/**
	 * Returns whether this is bitwise equal to the other vector. If other is not a V3D then this will always return false.
	 */
	@Override
	public boolean equals(Object other)
		{
		if(other instanceof V3D)
			return (((V3D)other).p[0]==p[0] && ((V3D)other).p[1]==p[1] && ((V3D)other).p[2]==p[2]);
		return false;
		}
	@Override
	public int hashCode()
		{
		return (int)(p[2]+p[1]*256+p[0]*65536);
		}
	/**Checks if this vector is a multiple of the other vector.
	 * Returns false if exactly one vector is zero.
	 * Returns true if both vectors are zero.*/
	public boolean isMultipleOf(V3D other)
		{
		if(isZero() ^ other.isZero())
			return false;
		if(isZero() && other.isZero())
			return true;
		if(p[0] == 0)
			{
			if(other.p[0] == 0)
				{
				//Both are on yz-plane
				if(p[1] == 0)
					return other.p[1] == 0;//Both are on z-line 
				else
					{
					double m = other.p[1]/p[1];
					return (Math.abs(other.p[2]-p[2]*m) < Double.MIN_VALUE*10);
					}
				}
			else //This is on yz-plane, but other is not
				return false;
			}
		else
			{
			double m = other.p[0]/p[0];
			return (Math.abs(other.p[1]-p[1]*m) < Double.MIN_VALUE*10 && Math.abs(other.p[2]-p[2]*m) < Double.MIN_VALUE*10);
			}
		}
	/**Checks if all components are exactly zero.*/
	public boolean isZero()
		{
		return p[0]==0 && p[1]==0 && p[2]==0;
		}
	/**Sets this vector to the vector from 'from' to 'to', 
	 * then adds a multiple of orthogonalVector so that the
	 * resulting vector is orthogonal to it.
	 * orthogonalVector should be of length 1.
	 * Is not normalized.
	 * @return this vector*/
	public V3D setFromToOrthogonal(V3D from, V3D to, V3D orthogonalVector)
		{
		set(to);
		sub(from);
		double d = -dot(orthogonalVector);
		add(orthogonalVector,d);
		return this;
		}
	/**Returns the manhattan length.*/
	public double manLen()
		{
		return Math.abs(p[0])+Math.abs(p[1])+Math.abs(p[2]);
		}
	/**
	 * Optimal reversal of vector. Functionally equivalent to multiply(-1)
	 * @return This vector
	 */
	public V3D reverse()
		{
		p[0] = -p[0];
		p[1] = -p[1];
		p[2] = -p[2];
		return this;
		}
	/**Returns the distance from this vector to the other vector, 
	 * projected onto the plane specified by the normal and the location of this vector.
	 * This method involves heavy arithmetic.*/
	public double distancePlane(V3D other, V3D normal)
		{
		V3D projection = other.clone().sub(this);
		projection.add(normal, -projection.dot(normal));
		return projection.len();
		}
	/**Post-multiply a 3x3 matrix to transform this vector.
	 * @return this vector*/
	public V3D postMultiply(float[][] matrix)
		{
		Calc.rotationMatrixPostMultiplyOpt(matrix, p);
		return this;
		}
	/**Generate a random vector where each component ranges from -1 to 1.*/
	public static V3D manhattanRandom()
		{
		return new V3D(1-2*(double)Math.random(),1-2*(double)Math.random(),1-2*(double)Math.random());
		}
	/**Generate a random (approximately) unit vector using the fastest math available.
	 * The distribution is uniform across the surface of a sphere.*/
	public static V3D fastRandom()
		{
		float a = Calc.PI*2*Calc.random();
		float z = 1-2*Calc.random();
		float r = Calc.fastSqrt(1-z*z);
		return new V3D(r*Calc.fastCos(a), -r*Calc.fastSin(a),z);		
		}
	/**Generate a random unit vector.
	 * The distribution is uniform across the surface of a sphere.*/
	public static V3D unitRandom()
		{
		double a = Calc.PI*2*(double)Math.random();
		double z = 1-2*(double)Math.random();
		double r = (double)(Math.sqrt(1-z*z));
		return new V3D(r*(double)Math.cos(a), -r*(double)Math.sin(a),z);		
		}
	}
